#include "lib/pure_pursuit_controller.h"
#include <algorithm>
#include "nav2_util/node_utils.hpp"

using std::hypot;
using std::min;
using std::max;
using std::fabs;

PurePursuitControllerD::PurePursuitControllerD(rclcpp::Node::SharedPtr &node, std::shared_ptr<tf2_ros::Buffer> tf)
{
    node_ = node;
    tf_ = std::move(tf);
    clock_ = node_->get_clock();
    truncation_num = 20;
    max_angular_vel_ = 0.5;
    min_angular_vel_ = 0.4;
    desired_linear_vel_ = 0.2;
    lookahead_dist_ = 0.05;
    goal_tolerate_dis = 0.05;
    goal_tolerate_angle = 0.017453292 * 20;
    method = 1;
    transform_tolerance_ = rclcpp::Duration::from_seconds(1.0);
}


bool PurePursuitControllerD::insertPose(double len, geometry_msgs::msg::PoseStamped pose, nav_msgs::msg::Path &path)
{
    if (len < 1e-4) return true;
    if (path.poses.empty())
    {
        printf("Received plan with zero length");
        return false;
    }
    std::string base_frame = "base_link";            // /base_link
    std::string global_frame = path.header.frame_id; // /map
    geometry_msgs::msg::PoseStamped transformed_pose, pose_local;
    pose_local = geometry_msgs::msg::PoseStamped();
    pose_local.header.frame_id = base_frame;
    pose_local.header.stamp = path.header.stamp;
    transformPose(tf_, global_frame, pose_local, transformed_pose, transform_tolerance_);
    double dis = hypot(path.poses[0].pose.position.x - transformed_pose.pose.position.x,
                       path.poses[0].pose.position.y - transformed_pose.pose.position.y);
    int index = 0;
    while ((dis < len) && (index < int(path.poses.size() - 1)))
    {
        index++;
        dis = hypot(path.poses[index].pose.position.x - transformed_pose.pose.position.x,
                    path.poses[index].pose.position.y - transformed_pose.pose.position.y);
    }
    if (dis - len < 0.3) // 0.3m 车身直径
    {
        path.poses.erase(path.poses.begin(), path.poses.begin() + index);
        return true;
    }
    std::string frame = pose.header.frame_id;
    return true;
}


geometry_msgs::msg::PoseStamped PurePursuitControllerD::goalPose(bool global_frame, bool earse_this)
{
    auto pose = global_frame ? (global_path_remain.poses.size() > 0 ? global_path_remain.poses[0] : end_pose)
                             : goal_pose;
    if (earse_this && global_path_remain.poses.size() > 0)
    {
        global_path_remain.poses.erase(global_path_remain.poses.begin());
    }
    return pose;
}

bool PurePursuitControllerD::setNextGoal(geometry_msgs::msg::PoseStamped &pose_, double distance_sqr)
{
    if (global_path_remain.poses.size() > 0)
    {
        plan_in_robot_frame = transformGlobalPlan(global_path_remain);
        if (plan_in_robot_frame.poses.empty())
        {
            return false;
        }
        auto goal_pose_iter = std::find_if(plan_in_robot_frame.poses.begin(), plan_in_robot_frame.poses.end(),
                                           [&](const auto &plan_pose_)
                                           {
                                               return (pow(plan_pose_.pose.position.x, 2) +
                                                       pow(plan_pose_.pose.position.y, 2)) >= distance_sqr;
                                           });

        int point_size = std::distance(plan_in_robot_frame.poses.begin(), goal_pose_iter);
        if (goal_pose_iter != plan_in_robot_frame.poses.end())
        {
            if (point_size > 0)
            {
                global_path_remain.poses.erase(global_path_remain.poses.begin(),
                                               global_path_remain.poses.begin() + point_size);
            }
            pose_ = global_path_remain.poses[0];
            return true;
        } else
        {
            pose_ = end_pose;
            return true;
        }
    } else
    {
        pose_ = end_pose;
        return true;
    }
}


geometry_msgs::msg::TwistStamped PurePursuitControllerD::computeVelocityCommands(
        const geometry_msgs::msg::PoseStamped &pose, geometry_msgs::msg::Twist &velocity)
{
    static double rad_high1 = 0.017453292 * 20;
    static double rad_low1 = 0.017453292 * 6;
    static double rad_high2 = 0.017453292 * 30;
    static double rad_low2 = 0.017453292 * 6;
    static double rad = 0.017453292 * 8;
    static double goal_tolerate_angle_use;
    static bool turn = true;
    static double time0 = 0;
    double linear_vel, angular_vel, yaw;
    double use_linear_vel = desired_linear_vel_;
    double use_max_angular_vel = max_angular_vel_;
    double rad_high;
    double rad_low;
    if (method == 1)
    {
        rad_high = rad_high1;
        rad_low = rad_low1;
    } else if (method == 2)
    {
        rad_high = rad_high2;
        rad_low = rad_low2;
    }
    goal_tolerate_angle_use = goal_tolerate_angle > rad_low ? goal_tolerate_angle : rad_low;
    geometry_msgs::msg::TwistStamped cmd_vel;
    cmd_vel.header.frame_id = pose.header.frame_id;
    cmd_vel.header.stamp = clock_->now();

    if (global_path_remain.poses.empty())
    {
        return geometry_msgs::msg::TwistStamped();
    }

    plan_in_robot_frame = transformGlobalPlan(global_path_remain);
    if (plan_in_robot_frame.poses.empty())
    {
        return geometry_msgs::msg::TwistStamped();
    }

    auto goal_pose_iter = std::find_if(plan_in_robot_frame.poses.begin(), plan_in_robot_frame.poses.end(),
                                       [&](const auto &plan_pose_)
                                       {
                                           return hypot(plan_pose_.pose.position.x, plan_pose_.pose.position.y) >=
                                                  lookahead_dist_;
                                       });

    /* auto goal_pose_iter = std::find_if( plan_in_robot_frame.poses.begin(), plan_in_robot_frame.poses.end(),
          [&](const auto & plan_pose_) {
                return (hypot( plan_pose_.pose.position.x, plan_pose_.pose.position.y) >= lookahead_dist_ ||
                        fabs(atan2(plan_pose_.pose.position.y,plan_pose_.pose.position.x))>= rad_low);
              }); */
    int point_size = std::distance(plan_in_robot_frame.poses.begin(), goal_pose_iter);

    if (goal_pose_iter != plan_in_robot_frame.poses.end())
    {
        goal_pose = *goal_pose_iter;
        if (point_size > 0)
        {
            global_path_remain.poses.erase(global_path_remain.poses.begin(),
                                           global_path_remain.poses.begin() + point_size);
        }
    }
    else
    {
        RCLCPP_INFO(node_->get_logger(), " all pose distance < lookahead_dist , maybe finish path tracking!");
        std::cout << "global_path_remain piont= " << global_path_remain.poses.size() << std::endl;
        goal_pose = plan_in_robot_frame.poses[global_path_remain.poses.size() - 1];
        global_path_remain.poses.erase(global_path_remain.poses.begin(), std::prev(global_path_remain.poses.end()));
        double remain_dis = hypot(goal_pose.pose.position.x, goal_pose.pose.position.y);
        tf2::Quaternion tf_q;
        tf_q.setX(goal_pose.pose.orientation.x);
        tf_q.setY(goal_pose.pose.orientation.y);
        tf_q.setZ(goal_pose.pose.orientation.z);
        tf_q.setW(goal_pose.pose.orientation.w);
        double reamin_angle = tf2::getYaw(tf_q);
//      double reamin_angle= tf2::getYaw(goal_pose.pose.orientation);
        std::cout << "remain_dis= " << remain_dis << " reamin_angle= " << reamin_angle << std::endl;
        if (remain_dis <= goal_tolerate_dis && fabs(reamin_angle) <= goal_tolerate_angle_use)
        {
            last_turn_finished = true;
            global_path_remain.poses.clear();
            return geometry_msgs::msg::TwistStamped();
        }
    }

    int size_point = global_path_remain.poses.size();
    std::cout << "global_path_remain piont= " << size_point << std::endl;
    if (size_point < 2) use_linear_vel = 0.1;
    else if (size_point < 3) use_linear_vel = 0.15;
    tf2::Quaternion tf_qs;
    tf_qs.setX(goal_pose.pose.orientation.x);
    tf_qs.setY(goal_pose.pose.orientation.y);
    tf_qs.setZ(goal_pose.pose.orientation.z);
    tf_qs.setW(goal_pose.pose.orientation.w);
    yaw = tf2::getYaw(tf_qs);
// yaw = tf2::getYaw(goal_pose.pose.orientation);
    if (method == 1)
    {
        std::cout << "yaw= " << yaw << std::endl;
        if (fabs(yaw) >= rad_high)
        {
            turn = true;
            time0 = 0;
        } else if (fabs(yaw) <= rad_low)
        {
            if (time0 < 1e-3)
                time0 = node_->now().seconds();
            turn = false;
        }
        if (time0 > 1e-3)
        {
            if (node_->now().seconds() - time0 < 0.2)
            {
                cmd_vel.twist.linear.x =
                cmd_vel.twist.angular.z = 0;
                velocity = cmd_vel.twist;
                return cmd_vel;
            }
        }
        if (turn)
        {
            angular_vel = yaw > 0 ? min_angular_vel_ : -min_angular_vel_;
            linear_vel = 0;
        } else
        {
            double scale = 1.3;  //  1.2:   1.3
            auto curvature = 2.0 * goal_pose.pose.position.y /
                             ((goal_pose.pose.position.x * goal_pose.pose.position.x +
                               goal_pose.pose.position.y * goal_pose.pose.position.y) * scale);
            linear_vel = use_linear_vel;
            angular_vel = use_linear_vel * curvature;
        }
    }
    else if (method == 2)
    {
        yaw = yaw + atan2(goal_pose.pose.position.y, goal_pose.pose.position.x);
        std::cout << "yaw= " << yaw << std::endl;
        if (fabs(yaw) >= rad_high)
        {
            turn = true;
            time0 = 0;
        } else if (fabs(yaw) <= rad_low)
        {
            if (time0 < 1e-3)
                time0 = node_->now().seconds();
            turn = false;
        }
        if (time0 > 1e-3)
        {
            if (node_->now().seconds() - time0 < 0.2)
            {
                cmd_vel.twist.linear.x =
                cmd_vel.twist.angular.z = 0;
                velocity = cmd_vel.twist;
                return cmd_vel;
            }
        }
        if (turn)
        {
            angular_vel = yaw > 0 ? min_angular_vel_ : -min_angular_vel_;
            linear_vel = total_err = last_dis_p = 0;
        } else
        {
            linear_vel = use_linear_vel;
            double dis_p = yaw;
            double dis_d = dis_p - last_dis_p;
            angular_vel = dis_p * 1.0 + dis_d * 0.1;
            last_dis_p = dis_p;
        }
    }
    else if (method == 3)
    {
        use_max_angular_vel = 0.6;
        if (abs(yaw) < rad)
        {
            double scale = 1.0;
            auto curvature = 2.0 * goal_pose.pose.position.y /
                             ((goal_pose.pose.position.x * goal_pose.pose.position.x +
                               goal_pose.pose.position.y * goal_pose.pose.position.y) * scale);
            linear_vel = use_linear_vel;
            angular_vel = use_linear_vel * curvature;
        } else
        {
            angular_vel = yaw > 0 ? 0.6 : -0.6;
            linear_vel = 0;
        }
    }
    cmd_vel.twist.linear.x = linear_vel;
    cmd_vel.twist.angular.z = max(-1.0 * use_max_angular_vel, min(angular_vel, use_max_angular_vel));
    velocity = cmd_vel.twist;
    return cmd_vel;
}

bool PurePursuitControllerD::FinishedVirtualWall(double distance)
{
    distance_ = distance;
    if (global_path_remain.poses.size() == 0 || plan_in_robot_frame.poses.size() == 0 || last_turn_finished)
    {
        last_turn_finished = false;
        return true;
    }

    geometry_msgs::msg::PoseStamped &pose_last = plan_in_robot_frame.poses[plan_in_robot_frame.poses.size() - 1];
    //std::cout<< "global_path_remain: " << global_path_remain.poses.size()<< " plan_in_robot_frame: " << plan_in_robot_frame.poses.size()<<std::endl;
    //std::cout<< "end pose: x= " << pose_last.pose.position.x<< " , y= " << pose_last.pose.position.y<<std::endl;
    //std::cout<< "goal pose: x= " << goal_pose.pose.position.x<< " , y= " <<goal_pose.pose.position.y<<std::endl;
    return false;
};

void PurePursuitControllerD::setPlan(const nav_msgs::msg::Path &path)
{
    // Transform global path into the robot's frame
    global_path_original = path;
    global_path_remain = path;
    end_pose = global_path_remain.poses.back();
    plan_in_robot_frame = transformGlobalPlan(global_path_remain);
}

nav_msgs::msg::Path PurePursuitControllerD::transformGlobalPlan(const nav_msgs::msg::Path &path)
{
    if (path.poses.empty())
    {
        printf("Received plan with zero length");
    }

    nav_msgs::msg::Path transformed_plan;
    transformed_plan.header.frame_id = "base_link"; // /base_link
    transformed_plan.header.stamp = path.header.stamp;

    // Helper function for the transform below. Converts a pose from global
    // frame to robot's frame
    auto transformGlobalPoseToRobotFrame = [&](const auto &plan_pose_)
    {
        geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose;
        stamped_pose.header.frame_id = "map"; // /map
        stamped_pose.pose = plan_pose_.pose;
        transformPose(
                tf_, transformed_plan.header.frame_id,
                stamped_pose, transformed_pose, transform_tolerance_);
        return transformed_pose;
    };

    if (path.poses.size() >= truncation_num)
        std::transform(
                path.poses.begin(), path.poses.begin() + truncation_num,
                std::back_inserter(transformed_plan.poses),
                transformGlobalPoseToRobotFrame);
    else
        std::transform(
                path.poses.begin(), path.poses.end(),
                std::back_inserter(transformed_plan.poses),
                transformGlobalPoseToRobotFrame);


    if (transformed_plan.poses.empty())
    {
        printf("Resulting plan has 0 poses in it.");
    }
    return transformed_plan;
}

bool PurePursuitControllerD::transformPose(const std::shared_ptr<tf2_ros::Buffer> tf, const std::string frame,
                                           const geometry_msgs::msg::PoseStamped &in_pose,
                                           geometry_msgs::msg::PoseStamped &out_pose,
                                           rclcpp::Duration &transform_tolerance)
{

    if (in_pose.header.frame_id == frame)
    {
        out_pose = in_pose;
        return true;
    }
    try
    {
        tf->transform(in_pose, out_pose, frame);
        return true;
    }
    catch (tf2::ExtrapolationException &ex)
    {
        auto transform = tf->lookupTransform(frame, in_pose.header.frame_id, tf2::TimePointZero);
        if ((rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) > transform_tolerance)
        {
            RCLCPP_ERROR(rclcpp::get_logger("tf_help"),
                         "Transform data too old when converting from %s to %s",
                         in_pose.header.frame_id.c_str(), frame.c_str());
            RCLCPP_ERROR(rclcpp::get_logger("tf_help"),
                         "Data time: %ds %uns, Transform time: %ds %uns",
                         in_pose.header.stamp.sec, in_pose.header.stamp.nanosec,
                         transform.header.stamp.sec, transform.header.stamp.nanosec);
            return false;
        }
        else
        {
            tf2::doTransform(in_pose, out_pose, transform);
            return true;
        }
    }
    catch (tf2::TransformException &ex)
    {
        RCLCPP_ERROR(rclcpp::get_logger("tf_help"), "Exception in transformPose: %s", ex.what());
        return false;
    }
    return false;
}